Accurate 3D maps from depth images and motion sensors via nonlinear Kalman filtering

This paper investigates the use of depth images as localisation sensors for 3D map building. The localisation information is derived from the 3D data thanks to the ICP (Iterative Closest Point) algorithm. The covariance of the ICP, and thus of the localization error, is analysed, and described by a Fisher Information Matrix. It is advocated this error can be much reduced if the data is fused with measurements from other motion sensors, or even with prior knowledge on the motion. The data fusion is performed by a recently introduced specific extended Kalman filter, the so-called Invariant EKF, and is directly based on the estimated covariance of the ICP. The resulting filter is very natural, and is proved to possess strong properties. Experiments with a Kinect sensor and a three-axis gyroscope prove clear improvement in the accuracy of the localization, and thus in the accuracy of the built 3D map.Comment: Submitted to IROS 2012. 8 page

On the Covariance of ICP-based Scan-matching Techniques

This paper considers the problem of estimating the covariance of roto-translations computed by the Iterative Closest Point (ICP) algorithm. The problem is relevant for localization of mobile robots and vehicles equipped with depth-sensing cameras (e.g., Kinect) or Lidar (e.g., Velodyne). The closed-form formulas for covariance proposed in previous literature generally build upon the fact that the solution to ICP is obtained by minimizing a linear least-squares problem. In this paper, we show this approach needs caution because the rematching step of the algorithm is not explicitly accounted for, and applying it to the point-to-point version of ICP leads to completely erroneous covariances. We then provide a formal mathematical proof why the approach is valid in the point-to-plane version of ICP, which validates the intuition and experimental results of practitioners.Comment: Accepted at 2016 American Control Conferenc

Navigating with highly precise odometry and noisy GPS: a case study *

For linear systems, the Kalman filter perfectly handles rank deficiencies in the process noise covariance matrix, i.e., deterministic information. Yet, in a nonlinear setting this poses great challenges to the extended Kalman filter (EKF). In this paper we consider a simplified nonlinear car model with deterministic dynamics, i.e., perfect odometry, and noisy position measurements. Simulations evidence the EKF, when used as a nonlinear observer, 1-fails to correctly encode the physical implications of the deterministic dynamics 2-fails to converge even for small initial estimation errors. On the other hand, the invariant (I)EKF, a variant of the EKF that accounts for the symmetries of the problem 1-correctly encodes the physical implications of the deterministic information 2-is mathematically proved to (almost) globally converge, with explicit convergence rates, whereas the EKF does not even locally converge in our simulations. This study more generally suggests the IEKF is way more natural than the EKF, for high precision navigation purposes

Symmetry-preserving nudging: theory and application to a shallow water model

One of the important topics in oceanography is the prediction of ocean circulation. The goal of data assimilation is to combine the mathematical information provided by the modeling of ocean dynamics with observations of the ocean circulation, e.g. measurements of the sea surface height (SSH). In this paper, we focus on a particular class of extended Kalman filters as a data assimilation method: nudging techniques, in which a corrective feedback term is added to the model equations. We consider here a standard shallow water model, and we define an innovation term that takes into account the measurements and respects the symmetries of the physical model. We prove the convergence of the estimation error to zero on a linear approximation of the system. It boils down to estimating the fluid velocity in a water-tank system using only SSH measurements. The observer is very robust to noise and easy to tune. The general nonlinear case is illustrated by numerical experiments, and the results are compared with the standard nudging techniques